/**
 * @file Point.h
 * @author  
 * @brief 点类
 * @version 1.0
 * @date 2020-07-23
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#pragma once
#include <Eigen/Eigen>
#include <iostream>

namespace behavior_controller
{
    //点数据类型基类
    template <typename DataType>
    class PointBase
    {
    public:
        typedef Eigen::Matrix<DataType, 2, 1> Pt2D;
        PointBase() : point_(Pt2D::Zero()) {}     //构造函数
        PointBase(const Pt2D &pt) : point_(pt) {} //构造函数
        template <typename... Members>
        PointBase(const Members &... d) : point_(DataType(d)...) //构造函数
        {
            if (sizeof...(Members) != 2)
                std::cout << "Invalid number of constructor arguments." << std::endl;
        }
        /**
         * @brief 计算欧式距离
         * 
         */
        double Distance(const PointBase &pt) const
        {
            
            return (point_ - pt.point_).norm();
        }

        double DistanceSquare(const PointBase &pt) const
        {
            Pt2D pt_tmp= point_ - pt.point_;

            return ( pow(pt_tmp(0),2) + pow(pt_tmp(1),2) );
        }
        /**
         * @brief Get the Point object
         * 
         * @return const Pt2D& 
         */
        const Pt2D &GetPoint() const { return point_; }

        /**
         * @brief Get the Point object
         * 
         * @return Pt2D& 
         */
        Pt2D &GetPoint() { return point_; }
        void SetPoint(DataType x, DataType y) {
            point_(0,0)=x ; point_(1,0)=y ;}

    private:
        Pt2D point_; //Eigen的点成员变量
    };

    template <typename DataType>
    class Point : public PointBase<DataType>
    {
    public:
        typedef typename PointBase<DataType>::Pt2D Pt2D;
        Point() {}                                                                 //构造函数
        Point(const DataType &x, const DataType &y) : PointBase<DataType>(x, y) {} //构造函数
        Point(const Pt2D &pt) : PointBase<DataType>(pt) {}                         //构造函数

        //获取坐标x 常量类型
        inline const DataType &x() const { return Point<DataType>::GetPoint().x(); }
        //获取坐标x
        inline DataType &x() { return Point<DataType>::GetPoint().x(); }
        //获取坐标y 常量类型
        inline const DataType &y() const { return Point<DataType>::GetPoint().y(); }
        //获取坐标y
        inline DataType &y() { return Point<DataType>::GetPoint().y(); }
    };

    typedef Point<double> PointF; //定义浮点型类

} // namespace behavior_controller